/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.assets.statemachine;

import edu.wpi.first.wpilibj.templates.assets.shooterstates.ZingStateMachine;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 *
 * @author Jarrod Risley
 */
public abstract class StateMachine {
    
    /*
     * Global Class Variables
     */
    private State currentState;
    private State snocker; // "snakes" through the list.
    
    public static boolean risingEdge;
    public static boolean fallingEdge;
    
    public static boolean fallingEdgeLower;
    
    public static int riseCounter = 0;
    public static int fallCounter = 0;
    
    // CONSTRUCTOR //
    
    
    /**
     * Constructs a StateMachine. Note that this is protected; make an instance
     * of this class ONLY in a subclass of StateMachine.
     */
    protected StateMachine() {
        
        init();
        
    } // end StateMachine
    
    
     // METHODS //
    
    /**
     * Initialize all States here.
     */
    protected abstract void init();
    
    /**
     * Sets the first state the StateMachine is instantiated with.
     * 
     * @param state The first state to run. It is strongly recommended that an
     * off state be sent in first.
     */
    protected final void setFirstState(State state) {
        
        setState(state);

    } // end setFirstState
    
    /**
     * Sets the current state of the StateMachine, forcefully ending the 
     * previous state in the process.
     * 
     * @param newState The State to switch to.
     */
    public final void setState(State newState) {
        
        System.out.println("Setting new state to " + newState);
        
        currentState = newState;
        runCurrentState();
        
    } // end setState
    
    /**
     * Sets the next state of the SateMachine. Only for this class.
     * 
     * @param nextState The Next State.
     */
    private void setNext(State nextState) {
        
        currentState = nextState;
        runCurrentState();
        
    } // end setNextState
    
    /**
     * Gets the current State.
     * 
     * @return The current state of the StateMachine.
     */
    public final State getState() {
        return currentState;
    } // end setState
    
    private void runCurrentState() {
        
        currentState.resetState();
        
        
        
        currentState.runFirst();
        
        do {
            
        ZingStateMachine.switchState = CommandBase.shooter.checkSwitch(1);
        ZingStateMachine.switchStateLower = CommandBase.shooter.checkSwitch(2);
        
        if (ZingStateMachine.switchState == true && ZingStateMachine.oldSwitchState == false) {
            
            ZingStateMachine.oldSwitchState = ZingStateMachine.switchState;
            riseCounter++;
            System.out.println("rise: " + riseCounter);
            risingEdge = true;
            fallingEdge = false;
            
        } else if (ZingStateMachine.switchState == false && ZingStateMachine.oldSwitchState == true) {
            
            ZingStateMachine.oldSwitchState = ZingStateMachine.switchState;
            fallingEdge = true;
            fallCounter++;
            System.out.println("fall: " + fallCounter);
            risingEdge = false;
            
        } else {
            
            risingEdge = false;
            fallingEdge = false;
        } // end if-else-if-else
        
        if (ZingStateMachine.switchStateLower = false && ZingStateMachine.oldSwitchStateLower == true) {
            
            ZingStateMachine.oldSwitchStateLower = ZingStateMachine.switchStateLower;
            fallingEdgeLower = true;
            
        } // end if
        
            currentState.run();
            
        } while (!currentState.isFinished());
        
        currentState.runLast();
        currentState.end();
        
        if (currentState.getNext() != null) {
            
            System.out.println("Setting next state to " + currentState.getNext());
            setNext(currentState.getNext());
            
        } else {
            System.out.println(currentState + " has no chained state.");
        } // end if-else
        
    } // end runCurrentState
    
} // end State Machine